#ifndef DORMANDPRINCEEMBEDDEDRK_HPP
#define DORMANDPRINCEEMBEDDEDRK_HPP

#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>
#include "norm.hpp"
#include "Function.hpp"

using namespace std;

class DormandPrinceEmbeddedRK : public TimeIntegrator
{
private:
    int n_steps, N = 6, p;
    double T, mu;
    vector<double> value;
    vector<vector<double>> result;
    Norm norm;
public:
    DormandPrinceEmbeddedRK() {}
    DormandPrinceEmbeddedRK(int n_, double time, vector<double> Val) : n_steps(n_), T(time), value(Val) {}
    ~DormandPrinceEmbeddedRK() {}
    void solver();
    void error();
};

void DormandPrinceEmbeddedRK::solver()
{
    ofstream outfile;
    outfile.open("output.m");
    outfile << "points = [";
    double k = 1.0 * T / n_steps;
    vector<double> U = value;
    result.push_back(U);
    auto epsilon = 1e-6;
    double t = 0;

    while (t < T)
    {
        k = min(k, T - t);
        auto y1 = f(U, t);
        auto y2 = f(U + k / 5 * y1, t + k / 5);
        auto y3 = f(U + k * (3.0 / 40 * y1 + 9.0 / 40 * y2), t + 3.0 / 10 * k);
        auto y4 = f(U + k * (44.0 / 45 * y1 - 56.0 / 15 * y2 + 32.0 / 9 * y3), 4.0 / 5 * k + t);
        auto y5 = f(U + k * (19362.0 / 6561 * y1 - 25360.0 / 2187 * y2 + 64448.0 / 6561 * y3 - 212.0 / 729 * y4), t + 8.0 / 9 * k);
        auto y6 = f(U + k * (9017.0 / 3168 * y1 - 355.0 / 33 * y2 + 46732.0 / 5247 * y3 + 49.0 / 176 * y4 - 5103.0 / 18656 * y5), t + k);
        auto y7 = f(U + k * (35.0 / 3168 * y1 + 500.0 / 1113 * y3 + 125.0 / 192 * y4 - 2187.0 / 6784 * y5 + 11.0 / 84 * y6), t + k);
        auto U1 = U + k * (35.0 / 384 * y1 + 500.0 / 1113 * y3 + 125.0 / 192 * y4 - 2187.0 / 6784 * y5 + 11.0 / 84 * y6);
        auto U2 = U + k * (5179.0 / 57600 * y1 + 7571.0 / 16695 * y3 + 393.0 / 640 * y4 - 92097.0 / 339200 * y5 + 187.0 / 2100 * y6 + 1.0 / 40 * y7);
        double E = sqrt(norm.twonorm((U1 - U2)) / N) / epsilon;
        if (E <= 1)
        {
            U = U2;
            outfile << U << endl;
            result.push_back(U);
            t += k;
        }
        k = 0.84 * pow(E, -1.0 / 4) * k;
    }
    outfile << "];" << endl;
    outfile << "scatter(points(:,1), points(:,2),\".\");" << endl;
    outfile.close();
}

void DormandPrinceEmbeddedRK::error()
{
    int n = result.size();
    // cout << result[n - 1] << endl;
    // cout << result[0] << endl;
    double error = norm.Inftynorm(result[n - 1] - result[0]);
    cout << error << endl;
}

TimeIntegrator* createDormand(int n_, int p_, double T_, vector<double> value_) { return new DormandPrinceEmbeddedRK(n_, T_, value_); }
#endif